#include "PoseGL.hpp"
#include "Utils.hpp"
#include <chrono>
#include <algorithm>
using namespace std::chrono_literals;

unsigned int LineIndex[48]={
  0, 1,   // Nose ->Neck
  1, 2,   // Neck ->RShoulder
  2, 3,   // RShoulder->RElbow
  3, 4,   // RElbow ->RWrist
  1, 5,   // Neck ->LShoulder
  5, 6,   // LShoulder->LElbow
  6, 7,   // LElbow ->LWrist
  1, 8,   // Neck ->MidHip
  8, 9,   // MidHip ->RHip
  9, 10,  // RHip ->RKnee
  10, 11, // RKnee ->RAnkle
  8, 12,  // MidHip ->LHip
  12, 13, // LHip ->LKnee
  13, 14, // LKnee ->LAnkle
  0, 15,  // Nose ->REye
  0, 16,  // Nose ->LEye
  15, 17, // REye ->REar
  16, 18, // LEye ->LEar
  14, 19, // LAnkle ->LBigToe
  19, 20, // LBigToe ->LSmallToe
  14, 21, // LAnkle ->LHeel
  11, 22, // RAnkle ->RBigToe
  22, 23, // RBigToe ->RSmallToe
  11, 24, // RAnkle ->RHeel
};

PoseGL::PoseGL()
{
}

PoseGL::~PoseGL()
{
}

void PoseGL::ShowPose(const std::array<Cood3D,25> & pose)
{ 
  std::lock_guard<std::mutex> lck(poseListMut);
  poseList=pose;
  float x,y,z;
  for(auto & i : pose)
  {
    x+=i.x;
    y+=i.y;
    z+=i.z;
  }
  objCenterX=x/pose.size();
  objCenterY=y/pose.size();
  objCenterZ=z/pose.size();
  for(auto & i : poseList)
  {
    i.x-=objCenterX;
    i.y-=objCenterY;
    i.z-=objCenterZ;
  }
  bufferRefreshFlag=true;
}

void PoseGL::Init(int width,int height)
{
	renderingProgram1 = Utils::createShaderProgram("glsl/vertShader.glsl", "glsl/fragShader.glsl");
	renderingProgram2 = Utils::createShaderProgram("glsl/linevertShader.glsl", "glsl/linefragShader.glsl");

	aspect = (float)width / (float)height;
	pMat = glm::perspective(1.0472f, aspect, 0.1f, 1000.0f);

	cameraX = 0.0f; cameraY = 0.0f; cameraZ = 2.0f;
  routVal=2.0f;
	setupVertices();
}

void PoseGL::Display()
{
	glClear(GL_DEPTH_BUFFER_BIT);
	glClearColor(0.0, 0.0, 0.0, 1.0);
	glClear(GL_COLOR_BUFFER_BIT);

	glUseProgram(renderingProgram1);

	mvLoc = glGetUniformLocation(renderingProgram1, "mv_matrix");
	projLoc = glGetUniformLocation(renderingProgram1, "proj_matrix");

	vMat = glm::translate(glm::mat4(1.0f), glm::vec3(-cameraX, 0.0f , -cameraZ));

	glUniformMatrix4fv(projLoc, 1, GL_FALSE, glm::value_ptr(pMat));

  std::lock_guard<std::mutex> lck(poseListMut);
	for (displayLoopi = 0; displayLoopi<25; displayLoopi++) {
		tMat = glm::translate(glm::mat4(1.0f), 
                          glm::vec3(poseList[displayLoopi].x, 
                                    poseList[displayLoopi].y,
                                    poseList[displayLoopi].z));
    sMat=glm::scale(glm::mat4(1.0f),glm::vec3(0.02f,0.02f,0.02f));
    rMat=glm::rotate(glm::mat4(1.0f),routVal,glm::vec3(0.0f,1.0f,0.0f));
    xflipMat=glm::rotate(glm::mat4(1.0f),glm::pi<float>(),glm::vec3(1.0f,0.0f,0.0f));
		mMat = xflipMat * rMat * tMat * sMat;
		mvMat = vMat * mMat;

		glUniformMatrix4fv(mvLoc, 1, GL_FALSE, glm::value_ptr(mvMat));

		glBindBuffer(GL_ARRAY_BUFFER, vbo[0]);
		glVertexAttribPointer(0, 3, GL_FLOAT, false, 0, 0);
		glEnableVertexAttribArray(0);

		glEnable(GL_DEPTH_TEST);
		glDepthFunc(GL_LEQUAL);

		glDrawArrays(GL_TRIANGLES, 0, 36);
	}
  //画线
	glUseProgram(renderingProgram2);
  if(bufferRefreshFlag)
  {
    setupLines();
    bufferRefreshFlag=false;
  }
  rMat=glm::rotate(glm::mat4(1.0f),routVal,glm::vec3(0.0f,1.0f,0.0f));
  xflipMat=glm::rotate(glm::mat4(1.0f),glm::pi<float>(),glm::vec3(1.0f,0.0f,0.0f));
	mvMat = vMat * xflipMat * rMat;
	mvLoc = glGetUniformLocation(renderingProgram2, "mv_matrix");
	projLoc = glGetUniformLocation(renderingProgram2, "proj_matrix");
	glUniformMatrix4fv(mvLoc, 1, GL_FALSE, glm::value_ptr(mvMat));
	glUniformMatrix4fv(projLoc, 1, GL_FALSE, glm::value_ptr(pMat));

  glBindBuffer(GL_ARRAY_BUFFER,vbo[1]);
  glVertexAttribPointer(1,3,GL_FLOAT,false,0,0);
  glEnableVertexAttribArray(1);
  glLineWidth(2);
  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,vbo[2]);
  glDrawElements(GL_LINES,48,GL_UNSIGNED_INT,0);
}

void PoseGL::setupVertices()
{
	float vertexPositions[108] =
	{
    -1.0f,  1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, -1.0f, -1.0f,
		1.0f, -1.0f, -1.0f, 1.0f,  1.0f, -1.0f, -1.0f,  1.0f, -1.0f,
		1.0f, -1.0f, -1.0f, 1.0f, -1.0f,  1.0f, 1.0f,  1.0f, -1.0f,
		1.0f, -1.0f,  1.0f, 1.0f,  1.0f,  1.0f, 1.0f,  1.0f, -1.0f,
		1.0f, -1.0f,  1.0f, -1.0f, -1.0f,  1.0f, 1.0f,  1.0f,  1.0f,
		-1.0f, -1.0f,  1.0f, -1.0f,  1.0f,  1.0f, 1.0f,  1.0f,  1.0f,
		-1.0f, -1.0f,  1.0f, -1.0f, -1.0f, -1.0f, -1.0f,  1.0f,  1.0f,
		-1.0f, -1.0f, -1.0f, -1.0f,  1.0f, -1.0f, -1.0f,  1.0f,  1.0f,
		-1.0f, -1.0f,  1.0f,  1.0f, -1.0f,  1.0f,  1.0f, -1.0f, -1.0f,
		1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f,  1.0f,
		-1.0f,  1.0f, -1.0f, 1.0f,  1.0f, -1.0f, 1.0f,  1.0f,  1.0f,
		1.0f,  1.0f,  1.0f, -1.0f,  1.0f,  1.0f, -1.0f,  1.0f, -1.0f
	};

	glGenVertexArrays(1, vao);
	glBindVertexArray(vao[0]);
	glGenBuffers(3, vbo);

	glBindVertexArray(vao[0]);

	glBindBuffer(GL_ARRAY_BUFFER, vbo[0]);
	glBufferData(GL_ARRAY_BUFFER, sizeof(vertexPositions), vertexPositions, GL_STATIC_DRAW);

  glBindBuffer(GL_ARRAY_BUFFER,vbo[2]);
  glBufferData(GL_ARRAY_BUFFER,sizeof(LineIndex),LineIndex,GL_STATIC_DRAW);
}

void PoseGL::setupLines()
{
	glBindVertexArray(vao[0]);
  glBindBuffer(GL_ARRAY_BUFFER,vbo[1]);
  glBufferData(GL_ARRAY_BUFFER,sizeof(float)*3*25,poseList.data(),GL_DYNAMIC_DRAW);
}

void PoseGL::Resize(int newWidth, int newHeight)
{
	aspect = (float)newWidth / (float)newHeight;
	glViewport(0, 0, newWidth, newHeight);
	pMat = glm::perspective(1.0472f, aspect, 0.1f, 1000.0f);
}
